//
// Created by ze on 2023/11/4.
//

#include "UPRE_MOTOR.h"
#include "gpio.h"
#include "odrive_can.h"
void Motor_Speed(float motor1,float motor2,float motor3,float motor4)
{
//    printf("vel1:%f,vel2:%f,vel3:%f,vel4:%f\r\n",motor1/200.f,motor2/200.f,motor3/200.f,motor4/200.f);

motor1 = Limit_MaxAndMin(motor1,2000,-2000);
motor2 = Limit_MaxAndMin(motor2,2000,-2000);
motor3 = Limit_MaxAndMin(motor3,2000,-2000);
motor4 = Limit_MaxAndMin(motor4,2000,-2000);

    static int i=1;
    if(i==1) {
        set_vel(&hcan1, 0x004, motor4/200.f, 0);
        set_vel(&hcan1, 0x003, motor3/200.f, 0);
        set_vel(&hcan1, 0x002, motor2/200.f, 0);
        set_vel(&hcan1, 0x001, motor1/200.f, 0);
        i=0;
    }
    else
    {
        set_vel(&hcan1, 0x001, motor1/200.f, 0);
        set_vel(&hcan1, 0x002, motor2/200.f, 0);
        set_vel(&hcan1, 0x003, motor3/200.f, 0);
        set_vel(&hcan1, 0x004, motor4/200.f, 0);
        i=1;
    }
}

//void Motor_Speed(int32_t motor1,int32_t motor2,int32_t motor3,int32_t motor4)
//{
//
//    Vesc_Set_ERPM(&hcan1,72,    motor1);
//    Vesc_Set_ERPM(&hcan1,116,   motor2);
//    Vesc_Set_ERPM(&hcan1,95,    motor3);
//    Vesc_Set_ERPM(&hcan1,92,    motor4);
//    Vesc_Set_ERPM(&hcan1,92,    motor4);
//    Vesc_Set_ERPM(&hcan1,72,    motor1);
//    Vesc_Set_ERPM(&hcan1,116,   motor2);
//    Vesc_Set_ERPM(&hcan1,95,    motor3);
//
//}
